#ifndef _SERVO_KEYA_H__
#define _SERVO_KEYA_H__

#include <motor/motor.h>

#include <std_msgs/msg/int8.hpp>
#include <std_msgs/msg/int16.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/u_int8.hpp>
#include <std_msgs/msg/u_int16.hpp>
#include <std_msgs/msg/u_int32.hpp>
#include <agv_srvs/srv/set_canopen_entry.hpp>

#define KEYA_ENABLE_A           0x030D2001
#define KEYA_ENABLE_VEL_A       0x030D2011
#define KEYA_ENABLE_ABSPOS_A    0x030D2031
#define KEYA_ENABLE_RELPOS_A    0x030D2041
#define KEYA_DISABLE_A          0x030C2001
#define KEYA_RESET_ERR_A        0x030C20F1
#define KEYA_RESET_POS_A        0x030D0000

#define KEYA_ENABLE_B           0x030D2002
#define KEYA_ENABLE_VEL_B       0x030D2012
#define KEYA_ENABLE_ABSPOS_B    0x030D2032
#define KEYA_ENABLE_RELPOS_B    0x030D2042
#define KEYA_DISABLE_B          0x030C2002
#define KEYA_RESET_ERR_B        0x030C20F2
#define KEYA_RESET_POS_B        0x030D0000


namespace ros_canopen_driver {

struct PDO_CACHE_KEYA
{ 
    std::atomic<int8_t>      mode{0};
    std::atomic<uint16_t>    error_code{0};
    std::atomic<int32_t>     current_pos{0};
    std::atomic<int16_t>     current_vel{0};
    std::atomic<uint32_t>    status_word{0};
    std::atomic<int16_t>     current{0};
    std::atomic<double>      timestamp{0.0};
};

class ServoKeya : public Motor
{
public:
    ServoKeya(std::shared_ptr<rclcpp::Node> node, const std::string name);
    ~ServoKeya();

    int Motor_Init();

    bool Motor_Enable(bool onFlag);

    bool Motor_InitParam();

    bool Motor_HomeMachine();

    bool Motor_InitVmode();

    bool Motor_InitPmode();

    bool Motor_SwitchMode(int8_t mode);

 
    void Motor_SetAcc(uint32_t acc);

    void Motor_SetDec(uint32_t dec);

    void Motor_SetPmodeVel(uint32_t vel);

    void Motor_SetTargetVelRad(double vel);

    void Motor_SetTargetVelEnc(int vel);

    void Motor_SetTargetPosRad(double pos);

    void Motor_SetTargetPosEnc(int pos);

    void Motor_ForkLift(bool up, int vel);


    MotorState Motor_GetState();

    double Motor_GetCurrentVel();

    double Motor_GetCurrentPos();

    double Motor_GetCurrent();

    uint16_t Motor_GetStatusWord();

    uint16_t Motor_GetErrCode();

    int8_t Motor_GetMode();


    void Motor_Brake(bool on);

    void Motor_Shutdown();

    bool Motor_Halt();

    bool Motor_Recover();

    double Motor_GetTimestamp() { return pc_.timestamp;}

    std::string Motor_Err2userErr(const uint16_t& motorErr);

private:
    // Canopen *pCanopen;
    SERVOMOTOR_PORT m_servo_motor;
    std::atomic<MotorState> motorState;

    struct PDO_CACHE_KEYA pc_;

    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int32>> set_vel_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int32>> set_pos_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int32>> set_control_word_pub;

    rclcpp::Subscription<std_msgs::msg::UInt16>::SharedPtr errcode_sub;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr  current_pos_sub;
    rclcpp::Subscription<std_msgs::msg::Int16>::SharedPtr  current_vel_sub;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr  status_word_sub;
    rclcpp::Subscription<std_msgs::msg::Int16>::SharedPtr  current_sub;

    rclcpp::Client<agv_srvs::srv::SetCanopenEntry>::SharedPtr set_entry_cln;
    
    void errcode_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr& msg);
    void current_pos_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg);
    void current_vel_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg);
    void status_word_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg);
    void current_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg);

    bool servo_isEnable();

    bool servo_isModeSet();
    
    void servo_modeSet(int8_t mode);

    int servo_encoderGet();

};

}
#endif
